/*******************************************************************************
Copyright:      Bupt
File name:      laser.c
Description:    激光
Author:         20th
Version：       1.0
Data:           2019/12/9
*******************************************************************************/
#include "laser.h"
#include "string.h"
//left对应AD0, right对应AD1, sideLeft对应AD2, sideRight对应AD3

#define ERROR_ON_LEFT       0             //激光与实际的偏差
#define ERROR_ON_RIGHT      0
#define ERROR_ON_SIDELEFT   0
#define ERROR_ON_SIDERIGHT  0
// #define DIS_BTW_LR 0.45

LASER laser_left;
LASER laser_right;
LASER laser_sideLeft;
LASER laser_sideRight;

// LASER_ADC laser_adc[AVERAGE_AMOUNT];
uint32_t laser_adc1[AVERAGE_AMOUNT*2]; // adc1的两个通道, 对应AD0和AD1
uint32_t laser_adc2[AVERAGE_AMOUNT];   // adc2的通道, 对应AD2
uint32_t laser_adc3[AVERAGE_AMOUNT];   // adc3的通道, 对应AD3

// PID_Struct laser_ypos_pid = {200,50,0,0,0,0,0,0.005};
// PID_Struct laser_xpos_pid = {700,50,0,0,0,0,0,0.005};


/**激光线性方程参数*/
void laser_calculate_kb(LASER *sensor){
  sensor->k_param = (sensor->FAR_distance - sensor->NEAR_distance) / (sensor->FAR_voltage - sensor->NEAR_voltage);
  sensor->b_param = sensor->FAR_distance - sensor->k_param * sensor->FAR_voltage;
  // uprintf("k is %f and b is %f\r\n", sensor->k_param, sensor->b_param);
}

void laser_adc_split_1(uint32_t* laser_adc, LASER* laser)
{
  memcpy(laser->ADC_value, laser_adc, AVERAGE_AMOUNT*sizeof(uint32_t));
}

void laser_adc_split_2(uint32_t* laser_adc, LASER* laser_1, LASER* laser_2)
{
  for(uint8_t i = 0; i < AVERAGE_AMOUNT; i++) {
    laser_1->ADC_value[i] = laser_adc[i*2];
    laser_2->ADC_value[i] = laser_adc[i*2+1];
  }
}

// /**j激光ADC三个通道分别取数*/
// void laser_adc_split_3(LASER* laser_l,LASER* laser_r,LASER* laser_s){

//   for (int i = 0; i < AVERAGE_AMOUNT; i++){
//     laser_l->ADC_value[i]=laser_adc[i].adc_l;
//     laser_r->ADC_value[i]=laser_adc[i].adc_r;
//     laser_s->ADC_value[i]=laser_adc[i].adc_s;
//   }
// }

/**激光计算距离*/
float laser_calculate_distance(LASER *sensor , Kal_Struct *kal_laser_distance , Kal_Struct *kal_laser_adc){
  float sum_up = 0;
  float distance;
  for (int i = 0; i < AVERAGE_AMOUNT; i++){
    sum_up += sensor->ADC_value[i];
  }
  sensor->ADC_final = (int)sum_up / AVERAGE_AMOUNT;
  sensor->ADC_final = (int)KalMan(kal_laser_adc, sensor->ADC_final);
  distance = sensor->ADC_final * sensor->k_param + sensor->b_param;
  distance = KalMan(kal_laser_distance, distance);
  return distance;
}
////TODO :激光计算x  y  angle 的方式
///**激光计算x*/
//float laser_calculate_x(){
//  float laser_x=0;
//  return laser_x;
//}
///**激光计算y*/
//float laser_calculate_y(){
//  float laser_y=0;
//  return laser_y;
//}
///**激光计算角度*/
//float laser_calculate_angle(){
//  float laser_angle=0;
//  return laser_angle;
//}

/**激光初始化:计算kb;打开DMA*/
void laser_init(){

  //TODO 待校准
#if useLaserLeft
  // V=60.5d-183.67
  // V=22.535d-118.93
  laser_left.FAR_distance = 180;//70;
  laser_left.FAR_voltage = 3937.37;//4051.33;
  laser_left.NEAR_distance = 5;//5;
  laser_left.NEAR_voltage = -6.255;//118.83;
  laser_calculate_kb(&laser_left);
#endif /* useLaserLeft */

#if useLaserRight
  // V=61.55d-351.9
  // V=22.73d-120.83
  laser_right.FAR_distance = 180;//70;
  laser_right.FAR_voltage = 3970.57;//3956.6;
  laser_right.NEAR_distance = 5;//5;
  laser_right.NEAR_voltage = -7.18;//-44.15;
  laser_calculate_kb(&laser_right);
#endif /* useLaserRight */

#if useLaserSideLeft
  // V=104.69d-46.429
  // V=31.798d-119.21
  laser_sideLeft.FAR_distance = 130;//40;
  laser_sideLeft.FAR_voltage = 4014.53;//4141.17;
  laser_sideLeft.NEAR_distance = 5;//2.5;
  laser_sideLeft.NEAR_voltage = 39.78;//215.3;
  laser_calculate_kb(&laser_sideLeft);
#endif /* useLaserSideLeft */

#if useLaserSideRight
  // V=104.42d-163.06
  // V=31.554d-114.36
  laser_sideRight.FAR_distance = 130;//40;
  laser_sideRight.FAR_voltage = 3987.14;//4013.74;
  laser_sideRight.NEAR_distance = 5;//2.5;
  laser_sideRight.NEAR_voltage = 43.41;//98;
  laser_calculate_kb(&laser_sideRight);
#endif /* useLaserSideRight */

  // if (HAL_ADC_Start_DMA(&hadc1,(uint32_t *)laser_adc, 3*AVERAGE_AMOUNT) != HAL_OK){
  //   uprintf("Laser DMA wrong!!!\r\n");
  //   while (1);
  // }

  if (HAL_ADC_Start_DMA(&hadc1, laser_adc1, 2*AVERAGE_AMOUNT) != HAL_OK) {
    uprintf("Laser1 DMA wrong!!!\r\n");
    while (1);
  }

  if (HAL_ADC_Start_DMA(&hadc2, laser_adc2, AVERAGE_AMOUNT) != HAL_OK) {
    uprintf("Laser2 DMA wrong!!!\r\n");
    while (1);
  }

  if (HAL_ADC_Start_DMA(&hadc3, laser_adc3, AVERAGE_AMOUNT) != HAL_OK) {
    uprintf("Laser3 DMA wrong!!!\r\n");
    while (1);
  }
}
/**激光执行函数: 获取三个激光的距离;计算x y angle*/
void laser_exe(){
  if(flag.chassis_laser_flag != 1){return;}

  laser_adc_split_2(laser_adc1, &laser_left, &laser_right);
  laser_adc_split_1(laser_adc2, &laser_sideLeft);
  laser_adc_split_1(laser_adc3, &laser_sideRight);
  // laser_adc_split_3(&laser_left,&laser_right,&laser_side);
#if useLaserLeft
  laser_left.distance   = laser_calculate_distance(&laser_left, &kal_distance_L, &kal_adc_L) + ERROR_ON_LEFT;
#endif
#if useLaserRight
  laser_right.distance  = laser_calculate_distance(&laser_right, &kal_distance_R, &kal_adc_R) + ERROR_ON_RIGHT;
#endif
#if useLaserSideLeft
  laser_sideLeft.distance   = laser_calculate_distance(&laser_sideLeft, &kal_distance_SL, &kal_adc_SL) + ERROR_ON_SIDELEFT;
#endif
#if useLaserSideRight
  laser_sideRight.distance  = laser_calculate_distance(&laser_sideRight, &kal_distance_SR, &kal_adc_SR) + ERROR_ON_SIDERIGHT;
#endif

//  chassis.laser_pos_x = laser_calculate_x();
//  chassis.laser_pos_y = laser_calculate_y();
//  chassis.laser_angle = laser_calculate_angle();

  flag.chassis_laser_flag = 0;
}
/**激光打印三个距离*/
void laser_print_distance(){
#if useLaserLeft
  uprintf(" L:%.2fcm",laser_left.distance);
#endif
#if useLaserRight
  uprintf(" R:%.2fcm",laser_right.distance);
#endif
#if useLaserSideLeft
  uprintf(" SL:%.2fcm",laser_sideLeft.distance);
#endif
#if useLaserSideRight
  uprintf(" SR:%.2fcm",laser_sideRight.distance);
#endif
  uprintf("\n");
}

// /**激光打印pos*/
// void laser_print_pos(){
//   uprintf("Laser_X:%6fm\r\n",chassis.vega_pos_x);
//   uprintf("Laser_X:%6fm\r\n",chassis.vega_pos_y);
//   uprintf("Laser_A:%6f\r\n",chassis.vega_angle);
// }

void laser_print_adc() {
#if useLaserLeft
  uprintf(" L:%4u", laser_left.ADC_final);
#endif
#if useLaserRight
  uprintf(" R:%4u", laser_right.ADC_final);
#endif
#if useLaserSideLeft
  uprintf(" SL:%4u", laser_sideLeft.ADC_final);
#endif
#if useLaserSideRight
  uprintf(" SR:%4u", laser_sideRight.ADC_final);
#endif
  uprintf("\r\n");

}

#if useLaserLeft
float laser_left_distance()
{
  return laser_left.distance;
}
#endif
#if useLaserRight
float laser_right_distance()
{
  return laser_right.distance;
}
#endif
#if useLaserSideLeft
float laser_sideleft_distance()
{
  return laser_sideLeft.distance;
}
#endif
#if useLaserSideRight
float laser_sideright_distance()
{
  return laser_sideRight.distance;
}
#endif
